#include "../../include/api/imuApi.h"
#include "../../include/driver/driver_usart.h"
#include "../../include/device/rtkDevice.h"
#include "../../include/tool/tool.h"

/* ecal 和 protocol */
#include "control.pb.h"
#include "localization.pb.h"
#include "imu.pb.h"
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>
#include <ctype.h>
#include <ecal/ecal.h>
#include <ecal/msg/protobuf/publisher.h>
#include <ecal/msg/protobuf/subscriber.h>
/*******************************************************************
 * @author: tianxiaohuahua
 * @date: 23.12
 * @description: C_imuApi 构造函数 用于对类的成员变量初始化赋值
 * @input: void
 * @return: NULL
*******************************************************************/
C_imuApi::C_imuApi(/* args */)
{   
    p_imuDevice = NULL;
    spdlog::info("C_imuApi init!");
}


/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.12
 * @description: C_imuApi 析构函数 用于对类清理空间
 * @input: void
 * @return: NULL
*******************************************************************/
C_imuApi::~C_imuApi()
{
   delete p_imuDevice;
   p_imuDevice = NULL;
}

/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.12
 * @description: initTask 基于基类虚函数集成方法，用于统一对参数初始化
 * @input: CONFIG_IMU imu的模块需要的硬件和线程定义信息
 * @return: int32 返回成功失败
*******************************************************************/
int32_t C_imuApi::initTask(CONFIG_IMU &config_imu)
{
    spdlog::info("..............................................................");
    spdlog::info("C_imuApi::initTask");
    task_name   = config_imu.config_task.task_name;
    task_period = config_imu.config_task.task_period;
    task_topic  = config_imu.config_task.task_topic;
    spdlog::info("C_imuApi task_name:'{}'", task_name);
    spdlog::info("C_imuApi task_period:'{}'", task_period);
    spdlog::info("C_imuApi task_topic:'{}'", task_topic);
    spdlog::info("C_imuApi nband:          '{}'",config_imu.config_serial_hardware.baud);
    spdlog::info("C_imuApi ndata_bits:     '{}'",config_imu.config_serial_hardware.data_bits);
    spdlog::info("C_imuApi nserial_device: '{}'",config_imu.config_serial_hardware.serial_device );
    spdlog::info("C_imuApi nstop_bits:     '{}'",config_imu.config_serial_hardware.stop_bits);
    spdlog::info("..............................................................");
    p_imuDevice = new C_imuDevice(config_imu.config_serial_hardware.serial_device.c_str(), config_imu.config_serial_hardware.baud);
    if(p_imuDevice == NULL){
        spdlog::error("C_imuApi initTask false!!!");
        return false;
    }
    return true;
}


/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: runLoopTask 基于基类虚函数集成方法，用于统一运行线程
 * @input: void
 * @return: void
*******************************************************************/
void C_imuApi::runLoopTask(void)
{
    spdlog::info("C_imuApi start impApi driver software");
    
    std::vector<uint8_t> imuData;
    uint32_t countDisplay = 0; // 用于打印延迟计数

    eCAL::protobuf::CPublisher<ImuProtoPakg::controlImu> ecalImuPublisher(task_topic);
    while(1){
        if(true == getImuDeviceData(m_imuStrBuffer)){ // 接收串口数据，如果接收到了数据开始解析数据
            int32_t recv = parseImuData(m_imuStrBuffer); // 解析数据
            // 把数据包装成protobuf
            ImuProtoPakg::controlImu imuMessage;// 创建一个protobuf消息对象
            imuMessage.set_roll_rad(AHRSData_Packet.Roll);
            imuMessage.set_pitch_rad(AHRSData_Packet.Pitch);
            imuMessage.set_heading_rad(AHRSData_Packet.Heading);
            imuMessage.set_rollspeed(AHRSData_Packet.RollSpeed);
            imuMessage.set_pitchspeed(AHRSData_Packet.PitchSpeed);
            imuMessage.set_headingspeed(AHRSData_Packet.HeadingSpeed);
            // 发布数据
            ecalImuPublisher.Send(imuMessage);

            countDisplay=21;
            if(countDisplay > 20){
                countDisplay = 0;
                spdlog::info("C_imuApi: Heading data'{}'", AHRSData_Packet.Heading);
            }
        }
    }
}

/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: getImuDeviceData 获取IMU的数据内容
 * @input: void
 * @return: void
*******************************************************************/
int32_t C_imuApi::getImuDeviceData(std::vector<uint8_t> &dataBuf)
{
    int32_t recv = p_imuDevice->getUsartData(dataBuf); // 接收串口的数据 
    if(recv == RECV_DONE){
        // spdlog::info("recv done,recv len is {}", dataBuf.size());
        // for(int i=0; i<dataBuf.size(); i++){
        //     printf("%02x ", dataBuf[i]);
        // } printf("\r\n");
        return true;
    }else if(recv == REV_ERROR){ // 如果串口返回错误，说明串口出现了严重问题，可能是设备掉线
        while(1) {
            sleep(2);
            int32_t recv = p_imuDevice->getUsartData(dataBuf); // 尝试重新启动串口
            if(recv != REV_ERROR) break;
            spdlog::error("read imu serial error!");
        }
    } 
    return false;
}

/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: parseImuData 解析IMU的数据
 * @input: void
 * @return: void
*******************************************************************/
int32_t C_imuApi::parseImuData(std::vector<uint8_t> &dataBuf)
{
    int32_t recv = protocolDataCheck(dataBuf); // 对接收到的数据先进行检测，检测数据没问题后再进行解析。
    if(recv == RECV_IMU){ 
        std::vector<uint8_t> rximu;
        rximu = dataBuf;
        IMUData_Packet.gyroscope_x=DATA_Trans(rximu[7],rximu[8],rximu[9],rximu[10]);  //角速度
        IMUData_Packet.gyroscope_y=DATA_Trans(rximu[11],rximu[12],rximu[13],rximu[14]);
        IMUData_Packet.gyroscope_z=DATA_Trans(rximu[15],rximu[16],rximu[17],rximu[18]);
        IMUData_Packet.accelerometer_x=DATA_Trans(rximu[19],rximu[20],rximu[21],rximu[22]);  //线加速度
        IMUData_Packet.accelerometer_y=DATA_Trans(rximu[23],rximu[24],rximu[25],rximu[26]);
        IMUData_Packet.accelerometer_z=DATA_Trans(rximu[27],rximu[28],rximu[29],rximu[30]);
        IMUData_Packet.magnetometer_x=DATA_Trans(rximu[31],rximu[32],rximu[33],rximu[34]);  //磁力计数据
        IMUData_Packet.magnetometer_y=DATA_Trans(rximu[35],rximu[36],rximu[37],rximu[38]);
        IMUData_Packet.magnetometer_z=DATA_Trans(rximu[39],rximu[40],rximu[41],rximu[42]);  
        IMUData_Packet.Timestamp=timestamp(rximu[55],rximu[56],rximu[57],rximu[58]);   //时间戳
        // printf("IMU: The gyroscope_x =  %f\r\n",IMUData_Packet.gyroscope_x);
        // printf("IMU:The gyroscope_y =  %f\r\n",IMUData_Packet.gyroscope_y);
        // printf("IMU:The gyroscope_z =  %f\r\n",IMUData_Packet.gyroscope_z);
        // printf("IMU:The accelerometer_x =  %f\r\n",IMUData_Packet.accelerometer_x);
        // printf("IMU:The accelerometer_y =  %f\r\n",IMUData_Packet.accelerometer_y);
        // printf("IMU:The accelerometer_z =  %f\r\n",IMUData_Packet.accelerometer_z);
        // printf("IMU:The magnetometer_x =  %f\r\n",IMUData_Packet.magnetometer_x);
        // printf("IMU:The magnetometer_y =  %f\r\n",IMUData_Packet.magnetometer_y);
        // printf("IMU:The magnetometer_z =  %f\r\n",IMUData_Packet.magnetometer_z);
        // printf("IMU:The Timestamp =  %d\r\n",IMUData_Packet.Timestamp);
    } else if(RECV_AHRS == recv){
        std::vector<uint8_t> rxahrs;
        rxahrs = dataBuf;
        AHRSData_Packet.RollSpeed=DATA_Trans(rxahrs[7],rxahrs[8],rxahrs[9],rxahrs[10]);       //横滚角速度
        AHRSData_Packet.PitchSpeed=DATA_Trans(rxahrs[11],rxahrs[12],rxahrs[13],rxahrs[14]);   //俯仰角速度
        AHRSData_Packet.HeadingSpeed=DATA_Trans(rxahrs[15],rxahrs[16],rxahrs[17],rxahrs[18]); //偏航角速度
        AHRSData_Packet.Roll=DATA_Trans(rxahrs[19],rxahrs[20],rxahrs[21],rxahrs[22]);      //横滚角
        AHRSData_Packet.Pitch=DATA_Trans(rxahrs[23],rxahrs[24],rxahrs[25],rxahrs[26]);     //俯仰角
        AHRSData_Packet.Heading=DATA_Trans(rxahrs[27],rxahrs[28],rxahrs[29],rxahrs[30]);	 //偏航角
        AHRSData_Packet.Qw=DATA_Trans(rxahrs[31],rxahrs[32],rxahrs[33],rxahrs[34]);  //四元数
        AHRSData_Packet.Qx=DATA_Trans(rxahrs[35],rxahrs[36],rxahrs[37],rxahrs[38]);
        AHRSData_Packet.Qy=DATA_Trans(rxahrs[39],rxahrs[40],rxahrs[41],rxahrs[42]);
        AHRSData_Packet.Qz=DATA_Trans(rxahrs[43],rxahrs[44],rxahrs[45],rxahrs[46]);
        AHRSData_Packet.Timestamp=timestamp(rxahrs[47],rxahrs[48],rxahrs[49],rxahrs[50]);   //时间戳
        // printf("AHRS: The RollSpeed =  %f\r\n",AHRSData_Packet.RollSpeed);
        // printf("AHRS: The PitchSpeed =  %f\r\n",AHRSData_Packet.PitchSpeed);
        // printf("AHRS: The HeadingSpeed =  %f\r\n",AHRSData_Packet.HeadingSpeed);
        // printf("AHRS: The Roll =  %f\r\n",AHRSData_Packet.Roll);
        // printf("AHRS: The Pitch =  %f\r\n",AHRSData_Packet.Pitch);
        // printf("AHRS: The Heading =  %f\r\n",AHRSData_Packet.Heading);
        // printf("AHRS: The Quaternion.Qw =  %f\r\n",AHRSData_Packet.Qw);
        // printf("AHRS: The Quaternion.Qx =  %f\r\n",AHRSData_Packet.Qx);
        // printf("AHRS: The Quaternion.Qy =  %f\r\n",AHRSData_Packet.Qy);
        // printf("AHRS: The Quaternion.Qz =  %f\r\n",AHRSData_Packet.Qz);
        // printf("AHRS: The Timestamp =  %d\r\n",AHRSData_Packet.Timestamp);
    } else {
        return false;
    }
    return true;
}

// 协议数据校验c
/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.12
 * @description: protocolDataCheck 解析IMU数据
 * @input: dataBuf imu数据
 * @return: int32 返回成功失败
*******************************************************************/
int32_t C_imuApi::protocolDataCheck(std::vector<uint8_t> &dataBuf )
{
    uint32_t loadDataLen = 0;// 负载数据的长度
    std::vector<uint8_t> loadData;
    uint16_t crc16Data = 0;

    // spdlog::info("recv done,recv len is {}", dataBuf.size());
    // for(int i=0; i<dataBuf.size(); i++){
    //     printf("%02x ", dataBuf[i]);
    // } printf("\r\n");
    if(dataBuf[1] == TYPE_IMU){ // 寻找IMU的数据头
        loadDataLen = dataBuf[2]; // 得到负载部分的长度
        loadData.insert(loadData.end(), dataBuf.begin()+7, dataBuf.end()-1); // 得到负载部分
        if(IMU_LEN != loadDataLen){ // 根据协议IMU的固定数据长度一定为IMU_LEN
            spdlog::error("Imu load data is error!!");
            return REV_ERROR;
        }
        crc16Data = dataBuf[5]; // 计算 CRC部分
        crc16Data = (crc16Data << 8) + dataBuf[6];
        uint16_t recvCRC16 = CRC16_Table(loadData); // 计算得到负载部分的CRC
        if(recvCRC16 != crc16Data){ // 如果CRC校验不通过，说明数据负载部分的传输出现了错误，不能代表正确的数据
            spdlog::info("Imu load data is error, crc is wrong !");
            return REV_ERROR;
        }
        return RECV_IMU;
    } 
    
    else if(dataBuf[1] == TYPE_AHRS){
        loadDataLen = dataBuf[2]; // 得到负载部分的长度
        loadData.insert(loadData.end(), dataBuf.begin()+7, dataBuf.end()-1); // 得到负载部分
        if(AHRS_LEN != loadDataLen){ // 根据协议IMU的固定数据长度一定为IMU_LEN
            spdlog::error("AHRS load data is error!!");
            return REV_ERROR;
        }
        crc16Data = dataBuf[5]; // 计算 CRC部分
        crc16Data = (crc16Data << 8) + dataBuf[6];
        uint16_t recvCRC16 = CRC16_Table(loadData); // 计算得到负载部分的CRC
        if(recvCRC16 != crc16Data){ // 如果CRC校验不通过，说明数据负载部分的传输出现了错误，不能代表正确的数据
            spdlog::info("AHRS load data is error, crc is wrong !");
            return REV_ERROR;
        }
        return RECV_AHRS;
    }
    return false;
}